/*
 * elasticenergy.cpp
 *
 *      Author: Jerome RD Soine and Christoph A Brand
 *      Institution: Schwarz goup, Institute for Theoretical Physics, Heidelberg University, Germany
 */

#include "../../../../../../include/base/cell_models/ACM/energy/elastic/cableenergy.h"
#include "../../../../../../include/base/cell_models/ACM/geometry/base/vertex.h"
#include "../../../../../../include/base/cell_models/ACM/geometry/base/edge.h"

#include <math.h>
#include <iostream>
#include <list>

CableEnergy::CableEnergy(GeomObject* G, double SPRING_CONSTANT,
		double prestrain) :
		ElasticEnergy(G, SPRING_CONSTANT, prestrain) {
	;
}

double CableEnergy::getEnergy() {
	double energy = 0;
	vector<Edge*> edges = *g->getEdges();
	for (unsigned int i = 0; i < edges.size(); i++) {
		energy += getEnergyOfEdge(edges[i]);
	}
	return energy;
}

double CableEnergy::getEnergyOfEdge(Edge * edge){
	double length = edge->toVector().abs();
	double restlength = edge->getRestlength();
	if(length>restlength)
		return 0.5 * spring_constant * (length - restlength)
			* (length - restlength);
	else return 0;
}

double CableEnergy::getForceOfEdge(Edge * edge){
	double length = edge->toVector().abs();
	double restlength = edge->getRestlength();
	if(length>restlength)
		return spring_constant * (length - restlength);
	else
		return 0;
}

Vertex CableEnergy::getGradient(Vertex & v) {
	Vertex gradient(0, 0, 0);
	vector<Edge*> connectedEdges = *v.getEdgesConnectingVertex();

	for (unsigned int i = 0; i < connectedEdges.size(); i++) {
		double length = connectedEdges[i]->toVector().abs();
		double restlength = connectedEdges[i]->getRestlength();
		if(length>restlength){
			double absGradientValue = spring_constant * (length - restlength);

			Vertex * head = connectedEdges[i]->getHead();
			Vertex direction = connectedEdges[i]->toVector();
			if(head != &v) direction = direction * (-1); // Gradient in direction of growing energy
			direction = direction / direction.abs();

			gradient += direction * absGradientValue;
		}
	}

	return gradient;
}

